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ABSTRACT 



This paper presents a method of optimal filter 
design for sampled data systems, based on the theory 
originally developed by R. E. Kalman. The first half 
of the paper deals with the theoretical development 
of mathematical models for linear, discrete dynamic 
processes and the optimal filter equations for such 
processes. The latter half discusses digital pro- 
gramming techniques for optimal filter design followed 
by two illustrative examples. 
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PREFACE 



During the past several decades a fair amount of 
theoretical effort has been devoted to the study of 
problems which are of a statistical nature. Not the 
least important is the class of problems in communica- 
tion and control which involves the separation of random 
signals from random noise, or the estimation of the 
states of a dynamic process based on noisy observations 
of a few of these states. 

In several papers written since I960, R. E. 
Kalman developed a theoretical approach for optimization 
of filters for the above mentioned class of problems. 
The theory is not all-embracing in that certain con- 
ditions must be satisfied before his technique can be 
employed. 

The intention of this paper is to present a method 
for optimal filter design for sampled data systems, 
based on Kalman’s approach. The first half of the 
paper deals with the theoretical development of mathe- 
matical model parameters for linear dynamic processes 
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and the optimal filter equations for such processes. 

The latter half discusses digital programming techniques 
for optimal filter design followed by two. illustrative 
examples . 
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CHAPTER I 



MODELS FOR RANDOM PROCESSES 

Before one can hope to achieve any amount of 
effective filtering, it is necessary that a fair amount of 
knowledge, about the physical phenomena to be observed, 
is known. For instance, if a sine wave buried in noise, 
is to be recovered, an apriori knowledge of the signal, 
i.e. frequency of the sine wave, is necessary. In addi- 
tion if the statistics of the noise are known then 
optimum filtering can be achieved. It therefore becomes 
necessary to make a study of the message (signal) 
process before the construction of a filter is attempted. 
To maintain generality we will henceforth only consider 
random signals with the added stipulation that these 
signals are produced by linear dynamic systems excited 
by white noise. 

1-1 LINEAR DYNAMIC' SYSTEMS (CONTINUOUS 
TIME) . 

Since we are concerned only with linear dynamic 
systems a brief review of linear differential equations is 



in order. 



A first order differential equation 



4^ 

cHr 



■f o(X - U 



(l.D 



has a solution (see Appendix I) 

= e^Xo + fe~* u(r)^r 

0 



where <2 ^ is the homogenous solution and 



is the particular solution. 

Consider now a set of first order differential 
equations, which define a linear dynamical systems 




- -f «) 



(1.3) 



or in vector notation 

X - f(i) x f d (t) u tt) 



(1.4) 



where x and u are 1 x n column vectors and F and D 
are n x n matrices. 

The solution (see Appendix I) to this set of 
equations is: 

x.(-d = e f(vt Xo + fe f ®^DbO «C£> Jr (1,5) 

0 
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or it may be written 



t 

2C(Jr) = Cb(t,t c )x(t 0 ) + f $ (t,r)D(Y) u(r)dT 

By definition we call the vector x the state of 
the system and u the input or control function. 

Since all states (xj) may not be observable we 
define the output of the system to be 

aCO - HW i (1 -‘ 



where ^(t) is a p vector and H(t) is a pxn matrix. 
If all states were observable then H would be equal to 
the identity matrix I . 

We can now represent the system in matrix block 
diagram form as shown in Fig. 1-1. 




Fig. 1-1 Matrix block diagram of a linear dynamic system. 

The integrator in Fig. 1-1 actually represents n 
integrators, one for each state of the system, while 
F(t) shows how the outputs of the integrators are fed 



i 
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back to the inputs of the various integrators. Perhaps 
a look at a simple 2-state system at this time might 



clarify Fig. 1-1. 

Given the linear dynamic system of Fig. 1-2, deter- 
mine F(t), D(t), and H(t). 




Fig. 1-2 A 2-state system 

We can immediately write down the equations for 
the system as: 

x,(t) = x 2 C±J 

x z (t) - - < **(£) + u.(tj 

and our observable state(s) 

aw = 

It is immediately obvious that 

I 



( 1 . 8 ) 

( 1 . 9 ) 

( 1 . 10 ) 



F« r 
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, D« = 
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) and 



thus giving the vector differential equation 



r * i 
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u 



( 1 . 11 ) 
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and 



y <y = [/ oj 

1-2 LINEAR DYNAMIC SYSTEMS (DISCRETE- 
TIME) . 

If we specify the equations of a linear dynamic 
system in the form of difference equations then they 
are easily mechanized on present day digital computers. 
With this in mind the scope of this paper will be 
directed towards discrete- time situations. 

In (1.6) we see that the continuous-time solution 
to a linear dynamic system is: 

z(t) = $(*)*<>) x(t,) + J D(rJ L£(t)dr 0»6) 

If u(T) is held constant over the interval of 
Integration then we obtain: 

X(t) = <J> X^(tv) + A (t ,-toj UW (1.13) 

where 

t 

A = j $ (t t r) D(r) dr (1.14) 

to 

or more conveniently 

x('tti) = (t th t)x(t) + A (*">*) id tO (i.i5) 



Xz 



( 1 . 12 ) 
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In (1.15) we assume a sampling period of one time 
unit. A block diagram of the linear discrete— dynamic 



system is shown in Fig. 1-3. 



u (tj 



Altfi) 







-/ delav^-^- 



*(t) 






HU) 



y (■*•) 



Fig. 1-3 Matrix block diagram of a linear discrete- 
dynamic system. 



1-3 DETERMINATION OF MODEL PARAMETERS. 

The matrix <|>(**<»t)oc curing in (1.6) and (1.13) is 
called the TRANSITION matrix and has the following 



properties: 

<3) (t©) i D ) - I (Identity Matrix) 

Q(t Z} t,)^(t,yto) — ^ (^2 ) "to) 

d § (t, to) = F(t) § (t } t 0 ) 
dt 



(1.16) 

(1.17) 

(1.18) 



(1.16) and (1.17) are fairly obvious and (1.18) is 
obtained by setting u(t) to zero and differentiating 
(1.6). These properties can be useful in checking the 
accuracy of analytic expressions for the Q matrix. 

If the F matrix is constant then the transition 

# * 
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matrix elements depend only on the time difference 
t-t 0 and can be calculated from the following expres- 
sion: 



is by the use of signal flow techniques and the appli- 
cation of unit impulses to the input of selected 
integrators. This method will be demonstrated else- 
where in this paper. 

The A matrix may be obtained by performing the 
integration in ( 1 . 14 ) or by the second method mentioned 

T 

above for the <J> matrix. 

1-4 THE GAUSS-MARKOV PROPERTY. 

A large number of physical phenomena possess the 
Markov property. Basically it means that the best 
estimate of a future state of a process can be made 
without the knowledge of all the past history of the 
process. In a strict sense it implies that the best 
estimate of a future state can be derived from the 
last observation of the states. A very trivial example 




(1.19) 



A second (and easier) method for obtaining $(t,t Q ) 
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would be the motion of a particle with a constant velocity 
vector. Given the best estimate of the present position 
and velocity of the particle one can formulate a best 
estimate of position and velocity for any time in the 
future. In fact the output from any linear dynamic 
system is Markovian. If u(t) is set equal to zero in 
(1.15) then this property may be expressed mathematically 
as: 

X (t + 0 s CO Ct-Hji) Xd) (1.20) 

If u(t) is a gaussian random vector then the sequence 
of random vectors . ..x(t-l), x(t/l), ... generated by 
( 1 . 15 ) is known as a gauss-Markov sequence . The stipu- 
lation that u(t) is gaussian implies that the sequence ..., 
u(t-l), u(t), u( t/1) , ... are normally distributed random 
vectors such that the cross-variance matrix: 

COV [u(t,) } u (t £ )J Z o fort,±T z (1.21) 

i.e. u(ti) and u(t2) are independent. In addition the 
random vectors are completely defined by specifying their 
first and second order moments, i.e. E(u(t) ) and 
E(u(t).u(t) ). . E or the purposes of this paper u(t) 
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will be assumed to have zero mean; 

i.e. £[u(t)J=0 for all t (1.22) 

E(u(t).u(t) ) is called the auto-covariance matrix 
of the vector u(t) and will be denoted by U(t), i.e; 

El LL® * iCt) T J = UCt) (1.23) 

or 

cov [ u ( t) j - UCt) 

Considering now the state of a process, we assume 
that the initial state, x(t Q ), is a gaussian random 
variable of zero mean and arbitrary variance. By 
repeated application of (1.15) > we see that future states, 
..., x(t-l), x(t), x(t/l), ... will also be gaussian 
random variables, since they are obtained by linear com- 
binations of gaussian random variables. 

In probability terminology we may now define the 
Gauss-Markov property. Since u(t^) and u(t£) are 
independent for tj ■£. tg f then the conditional probability 
distribution of x(t) is dependent only on the previous 
state, i.e: 

P (g(t) 6 2t|x(t-l),x (t-a),xtt-3) ; ) ( 1 . 24 ) 

- p (x(t; < ^ | x (t-o) 
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Where is an arbitrary vector. 

1-5 THE COMPLETE MODEL 

In Fig. 1-3 the observable output vector ^(t) cannot 
be measured with infinite accuracy and therefore to com- 
plete the model for random processes (with previously 
mentioned restrictions) , a source of measurement noise 
must be added. This is illustrated in Fig. 1-4 where v(t) 




Fig. 1-4 Model for random processes generated by discrete- 
time linear dynamic systems. 

as ^(t). v(t) is white noise (gaussian), which we assume 
to have zero mean with arbitrary variance: 

E [ If ( -t) 3 - 0 (1.25) 

£ [)/ (*) 'V(t) T j = COX/lvLt)] r R(tJ (1.26) 

In addition we specify that v(tj) and v(tg) are indepen- 
dent for tj^t2, i.e: 

C0V[v(t,), V(t a )] =0 -for t,jt L (1.27) 
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The output of our model is therefore _z(t), which 
contains the observable vector ^(t) corrupted by additive 
white noise, v(t)» 

H(t) - y (tj t 1 r(t) ( 1 . 28 ) 
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CHAPTER II 



THE KALMAN FILTER 

2-1 DEFINITION OF THE FILTERING PROBLEM 

In Chapter I, a model of a linear dynamic system, 
excited by white noise, was developed. The purpose of 
the Kalman filter is to give a best estimate of all states 
of the system, based on noisy observations of the 
observable states. Since the system is linear we may 
write 

xf'Ct) - X (t) + G(t) [2 (t) -git) ] (2.d 

where x*(t) is the best estimate of x(t), based on the 

current observation z_(t), 

s< 

x(t) is the best estimate of x(t), based on the 
previous observation _z(t-l), 

_z(t) is the best estimate of z(t), based on the 
previous observation z( t-1) , and, 

G(t) is an nxp gain matrix, the magnitude of its 
elements being indicative of the amount of information 
carried in z(t). 
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To solve the filtering problem, the filter must 
therefore determine values for the three unknowns on 
the right hand side of (2,1), namely x(t), _z(t), and G(t). 
2-2 SOLUTION OF THE FILTERING PROBLEM. 

Since we assume a complete knowledge of the 
dynamics of the system, computation of x(t) and £(t) ; is 
quite simple. 



X(t) = E 

= $ x A 7t-/) -J- ( 2 * 2 ) 



however since £ £u (f)J = 0 for all t then 

X (t) _ 2 C(t-I) 



(2.3) 



In the model we saw that 



E(t) = ^ (t) + ir(0 

= H(-t) x(t) + 



(1.28) 



now Z(t) - £[ Z(t) | f? (t-l)] 



= H (t) E (*) i i(t-0] + ELlstO] 

- H(t) x(t) 



(2.4) 



since a Lii-teJl = o 
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Having now developed expressions for x(t)'and _z(t), 
a matrix block diagram of the filter (see Fig. 2-1) can 
be produced. The only unknown yet to be calculated is 
the optimal gain matrix G(t). Before approaching this 
calculation a criterion for optimal must be specified. 




The criterion used is that we wish to find G(t) 
such that the loss function 

L - £ is minimized. (2.5) 

That is to say that the sum of the variances of the 
errors associated with the estimate of the individual 
states is minimized. Because the errors are gaussian 
it can be shown (ref. 1) that this criterion will in fact 
produce an optimal gain matrix. 

A number of different derivations for G(t) are 
available in the literature. For the most part these 
derivations are mathematically rigorous and somewhat 
complex. Perhaps the easiest one to follow is a semi- 

14 ‘ 



heuristic approach used by Schmidt, ^4^' • We wish to 
minimise the scalar loss function defined by (2.5) • Note 



that 

T 

~ ^~) T ( - True e £ i(X -x*)(X -X*)] (2.6) 

where the Trace operator denotes the sum of the main 
diagonal elements, and (t) is left- out to avoid unneces- 
sary clutter. 

Expanding the covariance matrix in (2.6), using 
(2.1), we obtain 

-**)(£ -%*) 3 3 £ (fa -Z)-G (g -|jj [(if -3) - 6 (S -i)fj 

-ffo -i )(s -i) r 0 T ] + £[<9 fe -|Xs-f)V7 

but 2. = H % tlf ) And i £ = H & 

Substituting for z and J;, and noting that 
we obtain 

E[(x-x*)(x-x*) T ] c F [<x-£)cx'£) t ]-E[gH (X‘2mx-£j t ] 

- £ [(* -ito 'X) t h g] : - *h *■ * a* * 

( H (x-x)(X- X)Jtf4 w r )G r ] 

- P~GHP - PH‘6 r t G(HPft T +R)G T ( 2 . 7 ) 
where P* E [(X-^)(x-X) T j and R- E[v*]£ T J ■ 




We now wish to find an expression for G such that 
the trace of (2.7) is a minimum. Since the terms in 
(2.7) are matrices this could become a very arduous task. 
Let us consider for a moment that (2.7) is a scalar 
expression (i.e: the matrices are lxl in dimension), thus 
reducing the right hand side to: 

P - IG P H r + G i ( H P H r t R) 

We now differentiate with respect to G and set the 
resultant to zero obtaining 

- i PH T + 2 (HPH t + R)G = 0 
or G = P H T (H p H T -t R)~‘ (2.8) 

It can now be shown that (2.8) will in general 
provide the optimum gain matrix by letting 
C = G - P H T (h P H T f R)' 1 

or 6 - C + PH T CHPh r +8)"' (2.9) 

Simple substitution of (2.9) for G in (2.7) will 
reveal that the trace of (2.7) will be a minimum for 
C=0. Thus (2.8) provides us with the optimum gain 
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equation. 



Combining equations (2.7) and (2.8) results in an 
expression for the covariance matrix of the error in the 
filter’s estimate of the states of the system; 

£[U-X*)(X*X*; T ]= P-PH T (HPH T «-fi)*'tfP - PH r [PH r lHPH r tR)' , ] T 



0 






0 



nr 



1 






= Pit) - Pit) hih (tfft) fife) m ( 2 . 10 ) 

L 



In order to complete the filtering problem a recur- 
sive relation for the conditional covariance matrix, 
P(t,t-l) s must be derived. 

Recall that 

P{+,t -0 :£[(X(t)-|(t>t-/[)(^(t) 

but X('i) • $ + A(t,t-I) U.C'i-l) 

and = <| X? (t-‘ >*->) 

<t-l) t-/)) T 



<*> £ {(*-S)(^ T lf Hr a e[uV> 
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since the expected values of the cross-product terms are 



zero. 
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Define Q(t) = A(t (2.12) 

Combining (2.10), (2.11), and (2.12), we obtain 

A A -< 



t Q(t) 



(2.13) 



Equation (2.13) is called the variance equation which 
denotes the covariance of the error between the actual 
states x(t), and the predicted states x(t,t-l). 

Since ( 2 . 13 ). is recursive, an initial covariance matrix, 
P(t Q ), must be specified, and, since we assume that x(t Q ) 
is gaussian, with zero mean, our best estimate of x(t 0 ) is 
zero. Hence 

p(vo) - E [x (x 0 )‘X T (t 0 ) ] (2.12+) 



In determining P(t Q ), one will often find that the 
elements off the main diagonal will be zero, that is to 
say that the individual initial states are independent of 
one another. To illustrate this, perhaps a simple example 
will be helpful. Let us suppose that we are going to make 
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observations of the position of a particle in motion, and 
assume that the particle's velocity is constant but unknown. 
The two states of the system are position and velocity. 

Any knowledge one might have about one of the initial 
states will in no way assist in determining the other initial 
state. Hence the two states are initially independent inso- 
far as the observer is concerned. However, as more 
observations are made, the two states build up a depend- 
ency and the off-diagonal elements in P(t), generated by 
(2.13) , become non-zero. 

2-3 SUMMARY OP FILTER EQUATIONS. 



For convenience the equations for the optimal filter 
are grouped below: J 

F\{t) + <?(*) 



Gte) - PJO H r (t) [h (t)P(t)h T (t) t R(t )] 



- 1 



'A 






A 



g - H(t) x (t, t-/) " 

= X (t/c-l) f G(i)[z(-t) -j?(t>t-/)] 






I 

II 

III 

IV 

v J 






V i -1 jJ [K f. j A/ "A-pf tM&X 

c ; 






0 * _ 

c r 
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CHAPTER III 



FILTER DESIGN 

3-1 SOME PRELIMINARY CONSIDERATIONS. 

The equations for the optimal filter were derived in 
Chapter II, and are summarized in Section 2-3* Design 
of the optimal filter consists mainly of writing a suitable 
computer program to carry out the calculations indicated 
by the filter equations- I through V . . The input to the 
filter is the noisy observation vector, 2 s(t), and the out- 
put is the best estimate of the system state vector, 
x*(t) . 

One of the chief problems in carring out the filter 
computations is the determination of the inverse of the 
matrix found in equation II. If this matrix is singular, 
then the inverse does not exist. One must then resort 
to the calculation of a pseudo-inverse. The manner in 
which the pseudo-inverse is determined is shown in [ct ] . 
and will not be discussed further here. In either case 
the time required for inverse computation is relatively 
high, the time being roughly proportional to the cube of 
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the dimensionality of the matrix. On present day com- 
puters several seconds of computation time may be 
necessary to determine the inverse of a 4x4 matrix. 

This being the case, one can easily see that the sampling 
rate may be adversely affected. In therefore behoves 
one to make a close study of this matrix to see if com- 
putation time can be reduced. 

To illustrate, let us assume we are going to track am 
object in space, receiving position information only. The 
observable states are range, bearing, and elevation (R,©, 

Q), and we wish to determine best estimates of these 

* • * 

states along with their derivatives (R, ©, 0). Our state 
vector would then be set up as follows: 



"r 




xf 


R 




x2 


© 




x3 


© 


s 


x4 


0 




x5 


L. -4 




x6 



The H matrix would become 



H(t) 



100000 

001000 

ooooio 
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and hence the matrix ( HPH + R ) would be 3x3 bn 
dimension. 

Recalling that P(t) is symmetrical, computational 
reduction might be possible if we can assume 



E[(X ; (t) -£i -° 

-for L = 1,4 

and further that R(t) has non-zero elements along the 
main diagonal only. Making the above assumptions the P 
and R matrices would become 



P(t) = 



and R(t) = 



P ii 

p21 

0 

0 

0 

0 

rll 

0 

0 



pl2 

p22 

0 

0 

0 

0 

0 

r22 

0 



0 

0 

p33 

p43 

0 

0 

0 

0 

r33 



0 

0 

p34 

p44 

0 

0 



0 

0 

0 

0 

p55 

p65 



0 

0 

0 

0 

p56 

p66 



We would then find that 



( HPH + R ) 



-1 



pll + rll 
0 



p3 3 + r22 
0 



0 

0 

1 

P55 + r33 
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since the inverse of a matrix, having non-zero elements 
along the main diagonal only, is found simply by inverting 
the diagonal elements. 

Suppose now that rate information is also available 
so that measurements of all six states are made. If, 
in addition to the above assumptions, we can assume that 
the cross-variance elements in the P matrix, involving 
the even subscripted states, are also zero, then 



9 times. 

A further reduction might be realized in the given 
example by the use of three filters doing 2x2 matrix 
manipulations as opposed to one filter computing at the 
6x6 level. 

Another consideration is the time lag between input 
and output. In real time situations this could be of the 




where aii = pii + rii, i = 1,6 

thus reducing computational time by at least 6^/3 x2^ or 
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utmost importance. A. glance at the filter equations, I-V, 
indicates that if all five equations are computed after the 
input, _z(t), has been received, then a considerable time 
lag could ensue before the output is generated. On the 
other hand, if the transition matrix, Q (t,t-l) r is known 
at time t— 1, then equations I, II, III, and IV can be 
computed prior to receiving the input. The time delay in 
this case would only involve the time taken to compute 
V, which might be in the order of micro-seconds. 

3-2 FILTER FOR A STATIONARY PROCESS. 

It is to be noted that equations I and II do not 
involve the observations, _z(t). If the process, which we 
are trying to observe, is stationary, i.ej(|), H, Q, and 
R are constant matrices, then it will be found that the 
optimal gain matrix, G , will stabilize to a constant va,lue. 
This matrix could then be precomputed (prior to any 
observations), and the filter would be reduced to the 
relatively simple calculations indicated by III, IV, and V. 

A digital program, which computes the optimum 
gain matrix for a stationary dynamic process, has been 
written in FORTRAN, and is found in Appendix II. It 
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is completely general and can handle any system with up to 

twelve states. It is written as a subroutine to eliminate 

possible conflict in the naming of variables. Essentially 

# 

the subroutine carries out the iterations indicated by I 
and II until such time that no element of the gain matrix 
changes from its previous value by an amount more than 
.00001. If higher accuracy is desired, this constant can 
be easily changed to the degree of accuracy required. 
Further description on the usage of this program is found 
in the Appendix. 

3-3 THE GENERAL FILTER. 

In the general case, non-stationarity is assumed. 
Appendix III contains two programs, the first of which 
performs the computations indicated by the five filter 
equations after each observation. The second program^ 
allows for the case when the transition matrix is known 
before an observation is made and hence reduces the 
time lag (previously discussed) between out put and input. 
Further discription of the usage of these programs is 
contained in the Appendix. 
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CHAPTER IV 



ILLUSTRATIVE EXAMPLES 

This section of the paper will deal with two illustra- 
tive examples. The aim of the first example is primarily 
tutorial. A thorough discussion of the model will be given, 
followed by the design of an optimal filter. The second 
example will deal with the track smoothing of an anti-ship 
missile. A mathematical model of the missile in flight will 
be set up and a filter designed for optimum track smooth- 
ing. 

EXAMPLE I: 

Given the transfer function for a low pass filter in 

Fig. 1, a) determine all mathematical model parameters, 

and, b) design an optimal filter which will give a best 

estimate of the states in the filter. Assume that the 

excitation at the input (u(t)), and the additive noise 

(v(t)) at the output are gaussian and stationary. 

iv(t) 

V (t) 

Fig. 4-1 Low pass filter of Example I. 



U(t) . 


1 




(S-HXS t2) 
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Our first step is to convert Fig. 4-1 to a more 



convenient form for analysis as shown in Fig. 4-2. 




Fig. 4-2 Signal flow graph for Example I 
From Chapter I, equations (1.8) through (1.12), we see 
that x(t) = Fx(t) + Du(t) (4»1) 

or 
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We wish now to find the solution to (4«1) in the 
form given by (1.15). To do so we must assume that 
u(t) is piece- wise constant. 

The O and A matrics will be obtained by using signal 
flow techniques- and applying unit impulses at selected 
locations in the diagram as illustrated in Fig. 4-2. . 

From Fig. 4-2 we see that 
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Taking the inverse of the above and letting the sample 
interval be T we find 
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and, 

A (t tT>t) - 

The solution is now in the form of (1.15) 

x(ttr) = t) xct) f A 



r - e’ T t t e' ar 



__r -it 

e - e 



(4*3 ) 



(1.15) 



and the mathematical model is shown in Fig. 4-3, 




Figure 4-3 Mathematical model for Example I 

where H(t) is obviouslyji oj, since only one state, xj , is 

observed. 

b) Design of optimal filter 

We recall that optimum filtering is based upon a 

knowledge of the statistics of u(t) and v(t), and there- 

V -y 

fore we assume that ETu(t)»u (t)J and Ej^v(t)'v (t)Jare 
part of the problem statement. We now calculate the 
covariance matrix 

Q (ttT) - A (t +T\t) E [.ufr) * l/(0] A r (*+r>t) (4»4) 
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and R (t) r £ [ u(t) . ^ r (t)] = A,, 



(4.5) 



The only remaining task is to select an initial 
covariance matrix, P(t Q ), for our best estimates of 
the initial states of the model. The selection will depend 
to a great extent on a good knowledge of the problem. 

In this instance the best selection would probably be the 
main diagonal of Q(t) (previously calculated), with the 
off-diagonal terms set to zero. The off-diagonal terms 
are zero initially since knowledge of x:j(t Q ) (at the first 
measurement) will in no way provide any information 
about 2Cg(t 0 ) iexj(t 0 ) and x 2 (t Q ) are uncorrelated Inso- 
far as the observer is concerned. 

Since the system under study is stationary, the 
optimum gain matrix may be pre-determined. This 
entails the use of SUBROUTINE CONFIL in Appendix 
II. A sample period of 0.1 secs, is used. Using (4»3) 
and ( 4 . 4)9 we compute the covariance of the states » 
Q(t). The element q^ in Q(t) is a measure of the 
expected signal power. By making (measurement 

noise power) equal to various multiples of q^ (including 
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zero) we are able to study the behaviour of the gain 
matrix as a function of noise-.to-signal power. 

The curues in Figure 4—4 indicate how the optimum 
gain matrix elements behave as noise-to-signal power is 
increased. With N/S = 0 we see that G1 is equal to 
unity. We expect this since there is no measurement 
noise and hence the best estimate of the observable 
state is the measurement z(t) itself. However as the 
noise power is increased the gain element falls off and 
the filter starts to rely more on the predicted value 
and less on the observed value. The matrix element G2 
also falls off in a similar fashion, with x g(t) becoming 
less dependent on z(t) as the relative noise power 
increases. 

EXAMPLE II 

Problem Statement - It is known that the enemy's 

! 

main anti-ship weapon is an air-launched missile which is 
normally launched at a distance of 250 to 300 miles from 
target. After launch, it climbs to an altitude of 40,000 
ft., attains a speed of approximately , 1000 mph, and 
maintains this speed by use of a sustainer motor. When 
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within 25 miles of the expected location of target a 
search device is switched on which pinpoints the target 
and enables the missile to guide itself to target. A 
typical friendly ship at sea is fitted with a mono-pulse 
search radar system with a scan rate of 10 scans/min. 
The ship is fitted with a digital computer and target 
information is available to the computer. It is known 
that the probability distribution function of the 
accumulated error (in both x and y) is approximately _ 
normal with zero mean and 2 mile standard deviation. 

. Data accumulated on similar missiles indicate that, 
due to erratic thrust developed by the missile motor and 
average atmospheric turbulence, the velocity of the 
missile varies in a random fashion (approximately 
ga us si an) . The standard deviation of this randomness 
is about Z% of mean velocity. 

Design. a filter which Will determine best estimates 
of the missile’s position and velocity. Assume that 
attack is equi-likely from all directions. 

SOLUTION 

Our first step is to set up a mathematical model 
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which describes the dynamics of the missile. In the form 



x (*) - F (t) X (t) + DCt)a(-t-) 



Considering only the component In the si direction we 



obtain 

X, It) 



VX 






O t 

0 o 



X.CO 

x,(t) 



u Wi 



(4.6) 



where sci is the position on the x axis 

Xg is the velocity component in the x direction 
A similar vector equation would describe the 
dynamics in the y direction. 

As we have seen earlier the solution to the above 

equation is 

X(t) = <£ (t,t ( ) X (t.) + A ( 4 - 7 ) 



assuming is piecswise constant # 

We now must determine ^(tgft^). 

From ( 4 . 4 ) we obtain the model shown in Fig 3«5.» & 
simple 1 /s L plant. 
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Fig 3*5 Model for missle in Example II. 
The parameters 0 and A are 




Since we intend to sample at a rate of 10 times/ 

min. (scan rate of radar) then tg — ...tj = 6 seconds. 

or T = 1__ hrs. 
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The problem statement specified however that 
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To complete the model we require values for H(t) and 
R ( t ) . 6 



From the problem statement 



R (±) = n, = 2 2 - q.o 



6 



V 

* ■> 









and since we are observing position only 



H Ct) r [l o ] 



Summarising our model we have (Fig. I 4 ..S) 




Fig 1+.6 Model for Missle of Example II 

We are now ready to set up the filter » One 
requirement is the initial covariance matrix F(o). Since 
the missile can approach us from any direction with 
equal probability our best estimate for the inital states 
is zero for both position and velocity, ie 
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We therefore find that 




p(o) = £[2(o)-a r w] 



To determine this we must have a knowledge about 
the detection capability of the radar. A. standard search 
radar looking for an object at 40,000 ft* elevation may 
have an initial detection density functio n, as shown in 
Figure 4*7? in all directions in the x-y plane. 



Since the missile may approach from any direction 
let us assume that the standard deviation when con- 
sidering the x direction only is 100 miles. Similarly let 
us assume the velocity component on the x axis has a 
standard deviation of 500 mph. . We then find that 



Pd 




0 <r= 200 Range (miles) 

FIG. 4.7 Probability density function for inital 
detection 
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— — p 

Our last remaining task is to select a value for V (0) 
to provide numbers for the Q(o) matrix. Our best 
initial estimate of this quantity would be the pgg element 



in F(o) . 



Hence 



V J>) r ( 500 )* 




Subsequent values of V x (t) could be calculated by 
using x 2 ( t ) . 

We are now prepared to write a program for the 
optimal filter. 

Since the transition matrix is a constant but Q 
is variable (being a function of V x ) the 2nd program of 
Appendix III would apply. 

We now summarize taking into account the y com- 
ponent of direction. The flow chart for the filter 
calculations is shown in Appendix III. 
n (number of states) = 4 
p (number of observables) = 2 
T (sample interval) = 1/600 hrs. 
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x(t) = 



z(t) = 



z(t) = 



H(t) = 



S( t+T, 



% 22 (°) 




10 0 0 
0 0 10 



0 1 
0 0 
0 0 



0 0 
0 0 
1 T 
0 1 



= qi 4 .i f (o) = ( .02 x 500 ) 2 = 100 
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R(t) 



4 0 
0 4 



P(o) = 



( 100) 2 0 0 0 
0 ( 500) 2 0 0 
0 0 (lOO) 2 0 
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APPENDIX I 



LINEAR DIFFERENTIAL EQUATIONS 
Let us consider the solution of a 1st order dif- 
ferential equation by the use of an integrating factor 
(p), to make an exact differential. 

Given: 



dx 

dt 



+ ax 



u 



(1) 



Find: 



Solution: 



x ( t ) = f(x , u, a, t) 



Multiply (1) by p(t) and attempt to make the 
resulting equation an exact differential. We have 



dx 

P dt paX = PU 



or 



( 2 ) 



d , x dp 

3t (p:c) - 3t x + 



apx = pu 



and 

aa <px) “ (a? - ap ) x + pu (3) 

Considering the homogeneous part of the problem 
(i.e., u = 0), we can make (3) an exact differential 
by setting 



4.4 



(4) 



dp 



- ap = 0 



We may guess at a solution for (4) as 

P = P Q e at (5) 

and by substitution in (4)» verify that it is a solution. 
Applying this to (3) gives 



fjr (Po eat *) = (°) x + Po© atu 



( 6 ) 



Integrating 



*o[ 



Pn I eat;S " X 0 = f ^ 

'o 



= e ar u(r) dr] 



(7) 



Multiplying by e"’ c ‘ L ' gives 



—at 

x = e - x 



+ e- at f 
Jo 



e ar u(T) dr (8) 



or 



-at 

x - e x. 



+ J .-Kin dr 



(9) 



The first part of (9) represents the .homogeneous 
solution •while the latter part 



X P - 



e" a(t ~ r) u (r ) dr 
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represents the particular solution or convolution integral . 
Now let us consider a set of n of these 1st order 
differential equations 
dx. 

or 

x = Fx + Du (11) 



where x and u are 1 x n column vectors and F and D 
are n x n matrices. Multiplying (11) by the integrating 
factor p(t) f (a row vector) we obtain, after some 
manipulation 



d 

dt 



( p x ) = 




( 12 ) 



As before we assume a solution for the adjoint variable, 
p(t) as 

_ _ -Ft 

P - P Q e 

Substituting into (12) and integrating gives 
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p 



-Ft 



: - x 

— — -o 



= J e" Pr Du(r) drj 



Ft 

Multiplying both sides by e gives 



x 



Ft 



x 

— o 




e P(t ~ Y) 



Du (r) dr 



Often the above is expressed in the form 

x(tj) = $(t 1# t Q ) x(t Q )+ A(tj, t Q ) u(t 0 ) 

where 5 is the transition matrix or fundamental 
and A represents the distribution matrix with 
held constant at its value u(t Q ). 



(13) 



(14) 

(15) 

matrix, 

u(r) 
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APPENDIX II 



THE STATIONARY FILTER 



PURPOSE: 

The attached program can be used to determine the 
stabilized optimum gain matrix for the estimation of the 
states of a stationary process. 

USAGE: 

1. Calling Sequence 

CALL CONFIL (P, Q, R, TR, H, KN, KP, KBR, 



G) 

2 . Arguments 

a. P - initial covariance matrix of system states - 

dimension (12 x 12) 

b. Q - covariance matrix of states due to gaussian 

excitation - .dimen,sion (12 x 12) 

c. R - covariance matrix of measurement noise — 

dimension (12 x 12) 

d. TR - transition matrix of pro.cess - 

dimension (12 x 12) 

e. H - matrix which defines the observable states - 

dimension (12 x 12) 

f. KN — number of states in the system - 

dimension (scaler) 
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g. KP - number of observable states - 

dimension (scaler) 

h. KER — error indication (= 2 implies the inverse 

of a matrix could not be obtained) - 
dimension (1) 

i. G — optimal gain matrix — 

dimension (12 x 12) 

3* Accuracy: see Chapter III 

4. Equipment Configuration: CDC 1604 'with FORTRAN 

60 . 



5. Cautions to Users 

a. All arguments in the main program must be 
dimensioned the same as those in CONFIL . 

b. The main program should contain an ERROR TEST 
( see 2h. ) 

6. Flow chart showing Typical Usage (see Fig. 11-^1). 
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Fig. 11-1 Flow diagram showing typical usage 
of CONFIL 
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APPENDIX II (CONTINUED) 
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APPENDIX II II- SUBROUTINE CONFIL (CONTINUED) 
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APPENDIX II II- SUBROUTINE CONFIL (CONTINUED) 



C ( I » J ) = A ( I » J ) •!• B { I » J ) 
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•APPENDIX II II- SUBROUTINE CONFIL (CONTINUED) 
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APPENDIX II II- • SUBROUTINE CONFIL (CONTINUED) 



APPENDIX III 



GENERAL FILTERS 

PREAMBLE: Two programmes,- In subroutine form, are 

contained in this appendix. , The first programme carries 
out all filter computations after the input has been 
received at each sample instant. The time lag between 
output and input will therefore depend on the time taken 
for these computations. The second programme is 
designed for use when certain parameters are known (or 
at least a very good estimate of these parameters can 
be made) prior to receiving the input at each sample 
instant. A prior knowledge of these parameters, namely 
TR(t,t-l), H(t), R(t), Q(t) (defined below), enables a 
considerable reduction of the above-mentioned time lag 
between output and input. This is achieved by perform- 
ing most of the filter computations prior to receiving 
the input. 

A. SUBROUTINE BESTX 

PURPOSE: This subroutine will provide an optimum 

estimate of the state vector for any sampled-data 
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linear dynamic process (with twelve or less states) if 
both the process random excitation and corruptive 
measurement noise vectors have gaussian distributions. 



USAGES 

1. Calling Sequence 

CALL BESTX ( F, Q, R, TR, H, KN, KP, KER, 
G, XP, 2, X) 



2» Arguments 



a. P - initial covariance matrix of system states - 

dimension (12 x 12) 

b. Q - covariance matrix of states due to gaussian 

excitation - 
dimension (12 x 12) 



c. R — covariance matrix of measurement noise — 
dimension (12 x 12) 

da. TR — transition matrix of process - 
dimension (12 x 12) 

e # H - matrix which defines the observable states - 
dimension (12 x 12) 

f« KN — number of states in the system - 
dimension (scaler) 



g. KP - number of observables states - 
dimension (scaler) 



h. KER — error indication (=2 implies the inverse of a 
matrix could not be obtained) - 
dimension (1) 



i. G — optimal gain matrix — 
dimension (12 x 12) 
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j. XP - predicted estimate of process state vector — 

dimension 

k. 2 — observation vector — 

dimension (IS x 12) 

1. X - optimal estimate of process state vector 
(generated by filter) — 
dimension (12 x 12) 

3. Equipment Configuration: CDC 1604 with FORTRAN 



60 . 



4. Cautions to User: 

a. All arguments in the main program must be 
dimensioned the same as those in BBSTX 

b. The main program should contain an ERROR TEST 
(see. 2h, ) 

c. See attached flow chart depicting typical usage. 
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Star 




Fig. 111-1 Flow diagram showing typical usage 
of BESTX 



) 
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RETURN 00470 
KER = 2 . 00400 
END • 00490 
END 
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APPENDIX III III- SUBROUTINE BESTX ( CONT I NUED ) 



B. SUBROUTINE GANDPR 



PURPOSE: This subroutine will provide, for each 

sample instant, aa optimum gain matrix, and, the pre- 
dicted values of the process and the observation state 
vectors, if the process is linear and the random 
excitation and corruptive measurement noise vectors 
have gaussian distributions. 

USAGE: 

1. Calling Sequence 

CALL GANDPR (P, Q, R, TR, H, X, XP, ZP, 
KN, KP, KER, G) 

2. Arguments 

a. P - initial covariance matrix of system states - 

dimension (12 x 12) 

b . Q - covariance matrix of states due to gaussian 

excitation - dimension (12 x 12) 

c. R— covariance matrix of measurement noise — 

dimension (12 x 12) 

d. TR - transition matrix of process - 

dimension (12 x 12) 

e. H - matrix which defines the observable states - 

dimension (12 x 12) 
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f. X - optimal estimate of process state vector - 

dimension (12 x 12) 

g. XP — predicted estimate of process state vector — 

dimension (12 x 12) 

h. Z? - predicted estimate of observable state 

vector - 

dimension (12 x 12) 

i. KN - number of states in the system — 

dimension (scaler) 

j. KP - number of observable states - 

dimension (scaler) 

k. KER — error indication (= 2 implies the inverse 

of a matrix could not be obtained) - 
dimension (1) 

l. G - optimal gain matrix - 

dimension (12 x 12) 

3. Equipment Configurations CDC 1604 with FORTRAN 
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4. Cautions to User: 

a. All arguments in the main program must be 
dimensioned the same as those in GANDPR. 

b. The main program should contain an ERROR TEST 
( see 2h. ) 

c. See attached flow chart depicting typical usage. 
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Fig. 111-2 Flow diagram showing 

typical usage of GANDPR 

t 
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. APPENDIX III III- SUBROUTINE GANDPR ( CONT 1 NUED ) 



SUBROUTINE PROD (AsBjNsHjLsC) 
DIMENSION A(12»12)*B(12»12)*C(12fl2) 
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